#include <physics/ValueLoader.h>
#include <physics/Node.h>
#include <physics/NodeLoader.h>
#include <physics/World.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2World.h>
#include <Box2D/Dynamics/b2Fixture.h>
#include <Box2D/Dynamics/Contacts/b2Contact.h>
#include <Box2D/Collision/Shapes/b2Shape.h>
#include <lib3d/node/Node.h>
#include <physics/Contact.h>

namespace physics
{

Node::Node(World *world)
	: world(world)
	, bodyDef(new b2BodyDef())
	, sceneNode(NULL)
	, attachedToNode(false)
	, userData(NULL)
{
	world->AddNode(this);
}

Node::~Node()
{
	world->RemoveNode(this, false);
	if(body)
		world->b2world->DestroyBody(body);
}

std::auto_ptr <xml::ObjectANSI> Node::CreateLoader()
{
	return std::auto_ptr <xml::ObjectANSI> (new NodeLoader(this));
}

void Node::CreateDefaultBody()
{
	body = world->b2world->CreateBody(&b2BodyDef());
	body->SetUserData(this);
}
	
void Node::SetFixedRotation(bool flag)
{
	body->SetFixedRotation(flag);
}

void Node::SetRootNode(lib3d::node::Node *node)
{
	rootNode = node;
	if(OnSetRootNode(node))
		return;

	if(sceneNodeName.get_str().empty())
		return;

	sceneNode = node->FindNode(sceneNodeName, true);
	if(!sceneNode)
		THROW(Gluck(__FUNCTION__, "Node \"" + sceneNodeName.get_str() + "\" cannot be found"));
}

void Node::SetSceneNode(liba::lib3d::node::Node *node, bool _attachedToNode)
{
	sceneNode      = node;
	attachedToNode = _attachedToNode;
}

liba::lib3d::node::Node * Node::GetSceneNode() const
{
	return sceneNode;
}

void Node::UpdateKinematic()
{
	if(!attachedToNode || sceneNode == NULL)
		return;
	
	const lib3d::Vector nodePosition = sceneNode->GetGlobalPosition();
	const lib3d::Vector nodeRotation = sceneNode->GetRotation();
	
	body->SetTransform(b2Vec2(nodePosition.x / world->GetPixelsInMeter(), nodePosition.y / world->GetPixelsInMeter()), nodeRotation.z);
	
	UpdateContacts();
}
	
void Node::Update()
{
	// synchronize position
	if(attachedToNode && sceneNode)
		return;

	if(sceneNode)
	{
		const b2Vec2& pos = body->GetPosition();
		const float angle = body->GetAngle();
		
		const lib3d::Vector &oldPos = sceneNode->GetPosition();
		sceneNode->ChangePosition(lib3d::Vector(float(int(pos.x * world->GetPixelsInMeter())), float(int(pos.y * world->GetPixelsInMeter())), oldPos.z));

		const lib3d::Vector &oldRot = sceneNode->GetRotation();
		sceneNode->ChangeRotation(lib3d::Vector(oldRot.x, oldRot.y, angle));
	}
	
	UpdateContacts();
}

void Node::UpdateContacts()
{
	// clear old contacts
	contacts.clear();
	
	// getting current contact edge
	b2ContactEdge * contactEdge = body->GetContactList();
	
	// itterate ovel all edges
	while(contactEdge != NULL)
	{
		// if endge have contact
		if(contactEdge->contact != NULL)
		{
			// extract manifold
			b2WorldManifold manifold;
			contactEdge->contact->GetWorldManifold(&manifold);
			
			// extract all contact points
			for(int i = 0; i < contactEdge->contact->GetManifold()->pointCount; i++)
			{
				Vector2 point  = Vector2(manifold.points[i].x, manifold.points[i].y) * world->GetPixelsInMeter();
				Vector2 normal = Vector2(manifold.normal.x, manifold.normal.y);
				contacts.push_back(Contact(point, normal, contactEdge->contact->IsTouching(), static_cast<physics::Node*>(contactEdge->other->GetUserData())));
			}
			
			// if sensor contact
			if(contactEdge->contact->GetManifold()->pointCount == 0 && contactEdge->contact->IsTouching())
				contacts.push_back(Contact(Vector2(), Vector2(), true, static_cast<physics::Node*>(contactEdge->other->GetUserData())));
		}
		
		// next contact edge
		contactEdge = contactEdge->next;
	}
}
	
void Node::SetPosition(const Vector2 & position)
{
	body->SetTransform(b2Vec2(position.x / world->GetPixelsInMeter(), position.y / world->GetPixelsInMeter()), body->GetAngle());
	Update();
}

Vector2 Node::GetPosition() const
{
	b2Vec2 position = body->GetPosition();
	return Vector2(position.x, position.y) * world->GetPixelsInMeter();
}

void Node::SetRotation(float angle)
{
	body->SetTransform(body->GetPosition(), angle);
}

float Node::GetRotation() const
{
	return body->GetAngle();
}
	
int Node::GetContactsCount() const
{
	return contacts.size();
}

Contact Node::GetContact(unsigned int index) const
{
	if(index >= contacts.size())
		return Contact();
	
	return contacts[index];
}

b2World* Node::GetB2World() const
{
	return GetWorld()->b2world.get();
}

void Node::SetName(const Atom & newName)
{
	name = newName;
}
	
const Atom& Node::GetName() const
{
	return name;
}
	
void Node::AddForce(const Vector2 & point, const Vector2 & force)
{
	float pixInM = world->GetPixelsInMeter();
	body->ApplyForce(b2Vec2(force.x / pixInM, force.y / pixInM), b2Vec2(point.x / pixInM, point.y / pixInM));
}
	
void Node::AddCentralForce(const Vector2 & force)
{
	body->ApplyForceToCenter(b2Vec2(force.x / world->GetPixelsInMeter(), force.y / world->GetPixelsInMeter()));
}
	
void Node::AddTorque(float torque)
{
	body->ApplyTorque(torque);
}

void Node::AddLinearImpulse(const Vector2 & point, const Vector2 & impulse)
{
	float pixInM = world->GetPixelsInMeter();
	body->ApplyLinearImpulse(b2Vec2(impulse.x / pixInM, impulse.y / pixInM), b2Vec2(point.x / pixInM, point.y / pixInM));
}

void Node::AddAngularImpulse(float torque)
{
	body->ApplyAngularImpulse(torque);
}	
	
void Node::SetLinearVelocity(const Vector2 & velocity)
{
	body->SetLinearVelocity(b2Vec2(velocity.x / world->GetPixelsInMeter(), velocity.y / world->GetPixelsInMeter()));
}
	
void Node::SetAngularVelocity(float velocity)
{
	body->SetAngularVelocity(velocity);
}
	
Vector2 Node::GetLinearVelocity() const
{
	b2Vec2 velocity = body->GetLinearVelocity();
	return Vector2(velocity.x, velocity.y) * world->GetPixelsInMeter();
}
	
float Node::GetAngularVelocity() const
{
	return body->GetAngularVelocity();
}
	
void Node::SetMass(float mass)
{
	body->SetType(mass > 0.0f ? b2_dynamicBody : b2_staticBody);
	
	if(mass > 0.0f)
	{
		b2MassData massData;
		massData.center = body->GetLocalCenter();
		massData.mass   = mass;
		massData.I      = 2.0f * mass * b2Dot(massData.center, massData.center);
		
		body->SetMassData(&massData);
	}
}

float Node::GetMass() const
{
	return body->GetMass();
}

bool Node::TestPoint(const Vector2 &pos) const
{
	b2Body* body = 0;
	b2Fixture* fixtuer = 0;

	if ( !(body = this->GetBody()) || !(fixtuer = body->GetFixtureList()) )
		return false;

	while(fixtuer)
	{
		if ( fixtuer->TestPoint(b2Vec2( pos.x / world->GetPixelsInMeter(), pos.y / world->GetPixelsInMeter() )))
			return true;
		fixtuer = fixtuer->GetNext();
	}

	return false;
}

void Node::Release()
{
	world->RemoveNode(this);
}
	
bool Node::IsStatic() const
{
	return body->GetType() == b2_staticBody;
}
	
void Node::MakeStatic()
{
	body->SetType(b2_staticBody);
}

void Node::MakeDynamic()
{
	body->SetType(b2_dynamicBody);
}

void Node::DisableGravity()
{
	body->SetGravityScale(0.0f);
}
	
void Node::EnableGravity()
{
	body->SetGravityScale(1.0f);
}

void Node::SetUserData(void * userData)
{
	this->userData = userData;
}

void * Node::GetUserData()
{
	return userData;
}
	
float Node::GetWorldPixelsInMeter() const
{
	return world->GetPixelsInMeter();
}

}	// namespace physics